﻿using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using NinjaToolbox.SubComponentModule;

namespace NinjaToolbox.PhysicsSystem
{
    [DisallowMultipleSubComponent]
    [DefaultExecutionOrder(-20)]
    public sealed class PhysicsObject : PhysicsBase, ITriggerObject
    {
        public enum ERegistOrder { OnEnable, Awake }

        [Serializable]
        public class OtherSettings
        {
            public float virtualRayLength = 10000f;
            public float movingEps = 0.03f;
            public float movingDetectTimeSpacing = 0.1f;
            public float fallingHeadFixEps = 0.001f;
            public int gravityDeepIterateCount = 3;
            public float gravityMoveStep = 0.05f;
            public bool updateMovingDetect = true;
            public bool isAcitivityObject = false;
            public bool optimizeMode = false;
            public bool enabledBvhQuery = false;
            public int groundBvhID = 0;
            public int wallBvhID = 0;
            public ERegistOrder registOrder = ERegistOrder.OnEnable;
        }

        [Serializable]
        public class GroundSettings
        {
            public bool enabled = true;

            public bool useFootPoint;
            public Transform[] footPoints;
            public Vector3[] footPointsRelativeOffset = new Vector3[] { new Vector3(0, -0.2f, 0) };

            public PhysicsSystem_LayerConstInfo groundLayerConstInfo;
            [Range(0, 31)]
            public int groundLayer;

            public float groundDetectLineLength = 0.5f;
        }

        [Serializable]
        public class GravitySettings
        {
            public bool enabled;

            public PhysicsObjectGravitySettingPreset preset;

            public float gravityAcce = 0.2f;
            public float gravityAcceScale = 1f;
            public float gravityAcceLimit = 10f;
            public float gravityScale = 0.5f;

            [HideInInspector]
            public float currentGravityAcce;
        }

        [Serializable]
        public class FallingHeadFixSettings
        {
            public bool enabled;
            public Vector3 fixCenterOffsetPoint = new Vector3(0, 1.5f, 0);
            public float gradient = 1f;
            public float radius = 2f;
            public float minSpeed = 100f;
            public float maxSpeed = 200f;
            public float speedAreaTopExtern = 1f;
            public float speedAreaBottomExtern = 0f;
        }

        const int INVALID = -1;
        const int RIGISING_STATE = 1;
        const int FALLING_STATE = 2;
        const int MOVING_STATE = 4;
        const int ON_GROUND_STATE = 8;

        public GroundSettings groundSettings = new GroundSettings();
        public GravitySettings gravitySettings = new GravitySettings();
        public FallingHeadFixSettings fallingHeadFixSettings = new FallingHeadFixSettings();
        public OtherSettings otherSettings = new OtherSettings();

        public bool isDrawGizmos = true;
        public bool alwaysVisibleGizmos = true;

        FallingHeadDetector mFallingHeadDetector;

        int mInternalPhysicsState;
        Vector3 mCacheVelocity;
        Vector3? mLastPosition;
        Vector3 mCurrentFrameGroundNormal;
        float mMovingDetectLastTick;

        bool mFootPointShift;
        int mFootPointIndexLoop;

        BVHTreeContainer mWallBvhTreeContainer;
        BVHTreeContainer mGroundBvhTreeContainer;

        Vector3 mCacheCurrentFramePhysicsGravity;

        Vector3 GroundFootPoint
        {
            get
            {
                if (groundSettings.useFootPoint)
                {
                    if (mFootPointShift)
                        mFootPointIndexLoop = (mFootPointIndexLoop + 1) % groundSettings.footPoints.Length;
                    return groundSettings.footPoints[mFootPointIndexLoop].position;
                }
                else
                {
                    if (mFootPointShift)
                        mFootPointIndexLoop = (mFootPointIndexLoop + 1) % groundSettings.footPointsRelativeOffset.Length;
                    return transform.localToWorldMatrix.MultiplyPoint3x4(groundSettings.footPointsRelativeOffset[mFootPointIndexLoop]);
                }
            }
        }

        public bool IsOnGround { get { return (mInternalPhysicsState & ON_GROUND_STATE) == ON_GROUND_STATE; } }

        public Vector3 Velocity { get { return mCacheVelocity; } }
        public bool IsMoving { get { return (mInternalPhysicsState & MOVING_STATE) == MOVING_STATE; } }
        public bool IsRising { get { return (mInternalPhysicsState & RIGISING_STATE) == RIGISING_STATE; } }
        public bool IsFalling { get { return (mInternalPhysicsState & FALLING_STATE) == FALLING_STATE; } }

        public Vector3 CurrentFrameGroundNormal { get { return mCurrentFrameGroundNormal; } }
        public event Action OnGroundRaycastHited;

        /// <summary>
        /// Arg1 - Self, Arg2 - Other.
        /// </summary>
        public event Action<PhysicsObject, PhysicsObject> OnTriggerEnter;

        /// <summary>
        /// Arg1 - Self, Arg2 - Other.
        /// </summary>
        public event Action<PhysicsObject, PhysicsObject> OnTriggerStay;

        /// <summary>
        /// Arg1 - Self, Arg2 - Other.
        /// </summary>
        public event Action<PhysicsObject, PhysicsObject> OnTriggerExit;


        public void Update()
        {
            mCacheCurrentFramePhysicsGravity = Physics.gravity;

            UpdateGravity();
            UpdateMovingStateDetect();
            UpdateFallingHeadFixDetect();

            UpdateGroundDetection();
        }

        public void UpdateGravity()
        {
            if (!gravitySettings.enabled) return;
            if (IsOnGround)
            {
                gravitySettings.currentGravityAcce = 0;
                return;
            }

            var gravityAcce = gravitySettings.gravityAcce;
            var gravityAcceScale = gravitySettings.gravityAcceScale;
            var gravityAcceLimit = gravitySettings.gravityAcceLimit;
            var gravityScale = gravitySettings.gravityScale;

            if (gravitySettings.preset != null)
            {
                gravityAcce = gravitySettings.preset.gravityAcce;
                gravityAcceScale = gravitySettings.preset.gravityAcceScale;
                gravityAcceLimit = gravitySettings.preset.gravityAcceLimit;
                gravityScale = gravitySettings.preset.gravityScale;
            }

            gravitySettings.currentGravityAcce += gravityAcce;
            gravitySettings.currentGravityAcce *= gravityAcceScale;
            gravitySettings.currentGravityAcce = Mathf.Min(gravitySettings.currentGravityAcce, gravityAcceLimit);
            var currentGravity = (gravitySettings.currentGravityAcce * mCacheCurrentFramePhysicsGravity.normalized) + mCacheCurrentFramePhysicsGravity * gravityScale;
            currentGravity = currentGravity * Time.deltaTime;

            if (otherSettings.enabledBvhQuery)
            {
                PhysicsUtil.StepMove(this, currentGravity, PhysicsUtil.StepMoveOptions
                  .Create(stepLength: otherSettings.gravityMoveStep, intersectDeepIterateCount: otherSettings.gravityDeepIterateCount)
                  .SetBvh(mGroundBvhTreeContainer.id));
            }
            else
            {
                PhysicsUtil.StepMove(this, currentGravity, PhysicsUtil.StepMoveOptions
                 .Create(stepLength: otherSettings.gravityMoveStep, intersectDeepIterateCount: otherSettings.gravityDeepIterateCount));
            }
        }

        public bool IsOnGroundTest(Vector3? inclinePoint = null, float normalDot = 0.7f)
        {
            var cacheFootPointShift = mFootPointShift;
            mFootPointShift = false;
            var groundFootPoint = inclinePoint != null ? GetGroundPoint(inclinePoint.Value) : GroundFootPoint;
            mFootPointShift = cacheFootPointShift;

            var groundDetectPoint = groundFootPoint + groundSettings.groundDetectLineLength * Vector3.down;

            if (mGroundBvhTreeContainer != null)
            {
                var isHit = PhysicsUtil.Linecast_Bvh(groundFootPoint, groundDetectPoint, out PhysicsHit hit, otherSettings.groundBvhID);
                if (isHit && Vector3.Dot(hit.Normal, mCurrentFrameGroundNormal) > normalDot)
                    return true;
            }
            else
            {
                var colliderObjectList = PhysicsSystemManager.Instance.ColliderObjectList;
                for (int i = 0, iMax = colliderObjectList.Count; i < iMax; i++)
                {
                    var colliderObject = colliderObjectList[i];

                    if (colliderObject.Layer != groundSettings.groundLayer) continue;

                    var physicsShape = colliderObject.PhysicsShape;
                    var isContainPoint = physicsShape.IsContainPoint(groundDetectPoint);
                    var isNotContainOutsidePoint = !physicsShape.IsContainPoint(groundFootPoint);

                    if (isContainPoint && isNotContainOutsidePoint)
                    {
                        if (Vector3.Dot(mCurrentFrameGroundNormal, physicsShape.IntersectsTest_GetNormal(groundFootPoint, groundDetectPoint)) > normalDot)
                            return true;
                    }
                }
            }

            return false;
        }

        public void UpdateGroundDetection(Vector3? inclinePoint = null)
        {
            if (!groundSettings.enabled) return;

            mInternalPhysicsState = mInternalPhysicsState & (~ON_GROUND_STATE);

            var groundFootPoint = inclinePoint != null ? GetGroundPoint(inclinePoint.Value) : GroundFootPoint;
            var groundDetectPoint = groundFootPoint + groundSettings.groundDetectLineLength * mCacheCurrentFramePhysicsGravity.normalized;

            if (mGroundBvhTreeContainer != null)
            {
                var isHit = PhysicsUtil.Linecast_Bvh(groundFootPoint, groundDetectPoint, out PhysicsHit hit, otherSettings.groundBvhID);

                if (isHit)
                {
                    var physicsShape = hit.PhysicsShape;
                    var isNotContainOutsidePoint = !physicsShape.IsContainPoint(groundFootPoint);
                    if (isNotContainOutsidePoint)
                    {
                        var groundNormal = physicsShape.IntersectsTest_GetNormal(groundFootPoint, groundDetectPoint);

                        mCurrentFrameGroundNormal = groundNormal;
                        mInternalPhysicsState |= ON_GROUND_STATE;

                        if (OnGroundRaycastHited != null)
                            OnGroundRaycastHited();
                    }
                }
            }
            else
            {
                var colliderObjectList = PhysicsSystemManager.Instance.ColliderObjectList;
                for (int i = 0, iMax = colliderObjectList.Count; i < iMax; i++)
                {
                    var colliderObject = colliderObjectList[i];

                    if (colliderObject.Layer != groundSettings.groundLayer) continue;

                    var physicsShape = colliderObject.PhysicsShape;

                   var isHitGround = physicsShape.IntersectsTest(groundFootPoint, groundDetectPoint, out PhysicsHit physicsHit);
                    if (isHitGround)
                    {
                        mCurrentFrameGroundNormal = physicsHit.Normal;
                        mInternalPhysicsState |= ON_GROUND_STATE;

                        if (OnGroundRaycastHited != null)
                            OnGroundRaycastHited();

                        break;
                    }
                }
            }
        }

        public void UpdateMovingStateDetect()
        {
            if (!otherSettings.updateMovingDetect) return;

            if (Time.time - mMovingDetectLastTick >= otherSettings.movingDetectTimeSpacing)
            {
                mMovingDetectLastTick = Time.time;
            }
            else
            {
                return;
            }

            mInternalPhysicsState = mInternalPhysicsState & (~MOVING_STATE);
            mInternalPhysicsState = mInternalPhysicsState & (~RIGISING_STATE);
            mInternalPhysicsState = mInternalPhysicsState & (~FALLING_STATE);

            ForceUpdateVelocity();
            var velocity = mCacheVelocity;

            if (velocity.magnitude > otherSettings.movingEps)
            {
                mInternalPhysicsState |= MOVING_STATE;

                var y = velocity.y;
                if (y > 0)
                    mInternalPhysicsState |= RIGISING_STATE;
                else if (y < 0)
                    mInternalPhysicsState |= FALLING_STATE;
            }

            mLastPosition = transform.position;
        }

        public void ForceUpdateVelocity()
        {
            mCacheVelocity = transform.position - mLastPosition.GetValueOrDefault(transform.position);
        }

        public void UpdateFallingHeadFixDetect()
        {
            if (!fallingHeadFixSettings.enabled) return;

#if UNITY_EDITOR
            mFallingHeadDetector.Gradient = fallingHeadFixSettings.gradient;
            mFallingHeadDetector.Radius = fallingHeadFixSettings.radius;
            mFallingHeadDetector.FixCenterOffsetPoint = fallingHeadFixSettings.fixCenterOffsetPoint;
            mFallingHeadDetector.MinSpeed = fallingHeadFixSettings.minSpeed;
            mFallingHeadDetector.MaxSpeed = fallingHeadFixSettings.maxSpeed;
            mFallingHeadDetector.SpeedAreaBottomExtern = fallingHeadFixSettings.speedAreaBottomExtern;
            mFallingHeadDetector.SpeedAreaTopExtern = fallingHeadFixSettings.speedAreaTopExtern;
            mFallingHeadDetector.EPS = otherSettings.fallingHeadFixEps;
#endif

            mFallingHeadDetector.Update();
        }

        Vector3 GetGroundPoint(Vector3 inclinePoint)
        {
            if (groundSettings.useFootPoint)
            {
                var d = float.MaxValue;
                var point = Vector3.zero;
                for (int i = 0; i < groundSettings.footPoints.Length; i++)
                {
                    var footPoint = groundSettings.footPoints[i].position;
                    var currentDistance = Vector3.Distance(footPoint, inclinePoint);
                    if (currentDistance < d)
                    {
                        d = currentDistance;
                        point = footPoint;
                    }
                }

                return point;
            }
            else
            {
                var d = float.MaxValue;
                var point = Vector3.zero;
                var cacheMatrix = transform.localToWorldMatrix;
                for (int i = 0; i < groundSettings.footPointsRelativeOffset.Length; i++)
                {
                    var footPoint = cacheMatrix.MultiplyPoint3x4(groundSettings.footPointsRelativeOffset[i]);
                    var currentDistance = Vector3.Distance(footPoint, inclinePoint);
                    if (currentDistance < d)
                    {
                        d = currentDistance;
                        point = footPoint;
                    }
                }

                return point;
            }
        }

        void Awake()
        {
            mFallingHeadDetector = new FallingHeadDetector();
            mFallingHeadDetector.Gradient = fallingHeadFixSettings.gradient;
            mFallingHeadDetector.Radius = fallingHeadFixSettings.radius;
            mFallingHeadDetector.FixCenterOffsetPoint = fallingHeadFixSettings.fixCenterOffsetPoint;
            mFallingHeadDetector.MinSpeed = fallingHeadFixSettings.minSpeed;
            mFallingHeadDetector.MaxSpeed = fallingHeadFixSettings.maxSpeed;
            mFallingHeadDetector.SpeedAreaBottomExtern = fallingHeadFixSettings.speedAreaBottomExtern;
            mFallingHeadDetector.SpeedAreaTopExtern = fallingHeadFixSettings.speedAreaTopExtern;
            mFallingHeadDetector.EPS = otherSettings.fallingHeadFixEps;
            mFallingHeadDetector.Init(transform, transform);

            mFootPointShift = true;

            if (otherSettings.enabledBvhQuery)
            {
                mWallBvhTreeContainer = BVHTreeContainer.GetContainerWithID(otherSettings.wallBvhID);
                mGroundBvhTreeContainer = BVHTreeContainer.GetContainerWithID(otherSettings.groundBvhID);
            }

            if (otherSettings.registOrder == ERegistOrder.Awake)
            {
                PhysicsSystemManager.Instance.RegistTriggerObject(this, this);
            }
        }

        void OnEnable()
        {
            if (otherSettings.registOrder == ERegistOrder.OnEnable)
            {
                PhysicsSystemManager.Instance.RegistTriggerObject(this, this);
            }
        }

        void OnDisable()
        {
            if (otherSettings.registOrder == ERegistOrder.OnEnable)
            {
                if (PhysicsSystemManager.Instance != null)
                    PhysicsSystemManager.Instance.UnregistTriggerObject(this);
            }
        }

        void OnDestroy()
        {
            if (otherSettings.registOrder == ERegistOrder.Awake)
            {
                if (PhysicsSystemManager.Instance != null)
                    PhysicsSystemManager.Instance.UnregistTriggerObject(this);
            }
        }

        void OnValidate()
        {
            if (groundSettings != null)
            {
                if (groundSettings.groundDetectLineLength < 0)
                    groundSettings.groundDetectLineLength = 0;
            }

            if (fallingHeadFixSettings != null)
            {
                if (fallingHeadFixSettings.gradient <= 0)
                    fallingHeadFixSettings.gradient = 0.001f;

                if (fallingHeadFixSettings.radius <= 0)
                    fallingHeadFixSettings.radius = 0.001f;
            }

            if (otherSettings != null)
            {
                if (otherSettings.fallingHeadFixEps < 0)
                    otherSettings.fallingHeadFixEps = 0f;

                if (otherSettings.gravityDeepIterateCount < 0)
                    otherSettings.gravityDeepIterateCount = 0;

                if (otherSettings.gravityMoveStep < 0)
                    otherSettings.gravityMoveStep = 0;

                if (otherSettings.virtualRayLength < 0)
                    otherSettings.virtualRayLength = 0;

                if (otherSettings.movingDetectTimeSpacing < 0)
                    otherSettings.movingDetectTimeSpacing = 0;
            }
        }

        void OnDrawGizmos()
        {
            if (!alwaysVisibleGizmos) return;

            DrawGizmos();
        }

        void OnDrawGizmosSelected()
        {
            if (alwaysVisibleGizmos) return;

            DrawGizmos();
        }

        void DrawGizmos()
        {
            if (!isDrawGizmos) return;

            DrawGroundDetectGizmos();
            DrawFallingHeadFixGizmos();
            PhysicsShape.DrawGizmos(PhysicsUtil.PHYSICS_SHAPE_GIZMOS_COLOR);
        }

        void DrawGroundDetectGizmos()
        {
            if (!groundSettings.enabled) return;

            var cacheGizmosColor = Gizmos.color;

            Gizmos.color = Color.blue;
            if (groundSettings.useFootPoint)
            {
                for (int i = 0; i < groundSettings.footPoints.Length; i++)
                {
                    var p0 = groundSettings.footPoints[i].position;
                    var p1 = p0 + groundSettings.groundDetectLineLength * -transform.up;
                    Gizmos.DrawLine(p0, p1);
                }
            }
            else
            {
                var cacheMatrix = transform.localToWorldMatrix;
                for (int i = 0; i < groundSettings.footPointsRelativeOffset.Length; i++)
                {
                    var p0 = cacheMatrix.MultiplyPoint3x4(groundSettings.footPointsRelativeOffset[i]);
                    var p1 = p0 + groundSettings.groundDetectLineLength * -transform.up;
                    Gizmos.DrawLine(p0, p1);
                }
            }

            Gizmos.color = cacheGizmosColor;
        }

        void DrawFallingHeadFixGizmos()
        {
            if (!fallingHeadFixSettings.enabled) return;

            if (!Application.isPlaying)
            {
                if (mFallingHeadDetector == null)
                {
                    mFallingHeadDetector = new FallingHeadDetector();

                    mFallingHeadDetector.Init(transform, transform);
                }

                mFallingHeadDetector.Gradient = fallingHeadFixSettings.gradient;
                mFallingHeadDetector.Radius = fallingHeadFixSettings.radius;
                mFallingHeadDetector.FixCenterOffsetPoint = fallingHeadFixSettings.fixCenterOffsetPoint;
                mFallingHeadDetector.MinSpeed = fallingHeadFixSettings.minSpeed;
                mFallingHeadDetector.MaxSpeed = fallingHeadFixSettings.maxSpeed;
                mFallingHeadDetector.SpeedAreaBottomExtern = fallingHeadFixSettings.speedAreaBottomExtern;
                mFallingHeadDetector.SpeedAreaTopExtern = fallingHeadFixSettings.speedAreaTopExtern;
                mFallingHeadDetector.EPS = otherSettings.fallingHeadFixEps;
            }

            mFallingHeadDetector.DrawGizmos();
        }

        #region --- ITriggerObject Members ---
        void ITriggerObject.ExecuteOnTriggerEnter(PhysicsObject triggeredObject)
        {
            OnTriggerEnter?.Invoke(this, triggeredObject);
        }

        void ITriggerObject.ExecuteOnTriggerStay(PhysicsObject triggeredObject)
        {
            OnTriggerStay?.Invoke(this, triggeredObject);
        }

        void ITriggerObject.ExecuteOnTriggerExit(PhysicsObject triggeredObject)
        {
            OnTriggerExit?.Invoke(this, triggeredObject);
        }
        #endregion
    }
}
